#include "../globals/globals.h"
#include "../pid/pid.h"

void fast_forward_vision_dist_relative_cb_static();

struct Arg_fast_forward_vision {
  bool run_done;  // 状态标志，是否完成
  double aim_dist_mm;
  double max_pwr;
  int timeout;
  bool waitForCompletion;
  double max_adjust_pwr;
} arg_fast_forward_vision;

bool fast_forward_vision_force_stop = false;
bool fast_forward_vision_running = false;
/////////////////////////////////////////////////////////////////////
void fast_forward_vision_stop() {
  fast_forward_vision_force_stop = true;
  while_wait(fast_forward_vision_running);
  fast_forward_vision_force_stop = false;
}
bool is_fast_forward_vision_done() { return arg_fast_forward_vision.run_done; }

///////////////////////////////////////////////////////////////////////////////
/*
启动函数
*/
///////////////////////////////////////////////////////////////////////////////
void fast_forward_vision_dist_relative(double aim_dist_mm, double max_pwr,
                                       int timeout, bool waitForCompletion,
                                       double max_adjust_pwr) {
  // 等待前一個线程结束
  while (fast_forward_vision_running) wait(5);

  arg_fast_forward_vision = {false,   aim_dist_mm,       max_pwr,
                             timeout, waitForCompletion, max_adjust_pwr};

  fast_forward_vision_force_stop = false;

  // 建立线程
  thread t(fast_forward_vision_dist_relative_cb_static);

  // 等待启动
  while (!fast_forward_vision_running) {
    wait(5);
  }

  // 等待完成
  if (waitForCompletion) {
    while (fast_forward_vision_running && !fast_forward_vision_force_stop)
      wait(20);
  }
}
////////////////////////////////////////////////////////////////////////////////
/*
线程执行函数
*/
void fast_forward_vision_dist_relative_cb_static() {
  fast_forward_vision_running = true;
  wait(10);  // cpu shift

  // 建立局部变量，供本线程使用
  double aim_dist_mm = arg_fast_forward_vision.aim_dist_mm;
  int timeout = arg_fast_forward_vision.timeout;
  double max_adjust_pwr = arg_fast_forward_vision.max_adjust_pwr;
  double max_pwr = arg_fast_forward_vision.max_pwr;
  ///////////////////////////////////////////////////
  int dist_beg_l = chassis.get_dist_mm(-1);

  int dir = 1;
  if (aim_dist_mm < 0) dir = -1;
  if (timeout == 0) timeout = 1000000;  // long time enough

  pid_ctrl pid = pid_ctrl::get_instance_old_version(0.5, 0.01, 0, 5, 0, 15, 10,
                                                    max_adjust_pwr);
  double turn_adj, fwd_adj;
  timer t;

  double rate = 0.1;  // 缓加速
  int dx;
  int dx_last = 0;

  while (!fast_forward_vision_force_stop && t.time() < timeout &&
         abs(chassis.get_dist_mm(-1) - dist_beg_l) < abs(aim_dist_mm)) {
    int vis_get_lowest_cdx(vision & v, signature & sig);
    if (auto_info.isAlliance(Alliance::blue))
      dx = vis_get_lowest_cdx(vis, vis_BLUE);
    if (auto_info.isAlliance(Alliance::red))
      dx = vis_get_lowest_cdx(vis, vis_RED);
    // cout << dx << endl;
    if (abs(dx_last - dx) > 15) {
      dx = 0;
      // cout<<"change too fast!!!!!!!\n";
    }
    if (abs(dx) > 40) {
      dx = 0;
      // cout<<"err too large!!!!!!!1\n";
    }

    turn_adj = -pid.pid_output_need_value(0, dx);
    fwd_adj = max_pwr - abs(turn_adj);

    // 缓加速
    fwd_adj *= rate;
    if (abs(fwd_adj) < 20) fwd_adj = sgn(fwd_adj) * 20;
    if (rate < 1) rate += 0.05;

    chassis.FT_set_voltage(dir * fwd_adj, turn_adj);
    wait(5);
    dx_last = dx;
  }
  chassis.mtr_stop(brakeType::hold);
  if (!fast_forward_vision_force_stop) wait(100);
  fast_forward_vision_running = false;
  arg_fast_forward_vision.run_done = true;
}